#!/usr/bin/env python
# -*- coding: utf-8 -*-

import cv2
import envs
import rospy
import actionlib

from darknet_ros_msgs.msg import CheckForObjectsAction, CheckForObjectsGoal


class Yolo():
    def __init__(self):
        self.yolo_client = actionlib.SimpleActionClient('darknet_ros/check_for_objects', CheckForObjectsAction)

        self.img_goal = CheckForObjectsGoal()
        self.img_goal.id = 3
        self.img_goal.image.width = 640
        self.img_goal.image.header = 480

    def active_callback(self):
        """
        action激活后回调（一次）
        """
        rospy.logdebug('joint trajectory action is actived')

    def done_callback(self, state, res):
        """
        action完成后回调（一次）
        """
        rospy.logdebug('joint trajectory action is finished')
        rospy.logdebug('state: %s', state)
        rospy.logdebug('res: %s', res)
        self.bboxes = res.bounding_boxes.bounding_boxes
        self.object_name = self.bboxes[0].Class

    def feedback_callback(self, feedback):
        """
        action执行过程中反馈的回调（多次）
        """
        rospy.logdebug('joint trajectory action is feedbacking...')
        rospy.logdebug('feedback: %s', feedback)

    def yolo_send_goal(self, goal):
        """
        action请求
        """
        self.yolo_client.wait_for_server()

        self.yolo_client.send_goal(goal, done_cb=self.done_callback, active_cb=self.active_callback, feedback_cb=self.feedback_callback)
        self.yolo_client.wait_for_result(rospy.Duration.from_sec(300.0))

    def detect(self):
        capture = cv2.VideoCapture(0)
        while(capture):
            ret, frame = capture.read()

        self.img_goal.image.data = frame
        self.yolo_send_goal(self.img_goal)
        rospy.loginfo('detect finished')
        return self.object_name


if __name__ == '__main__':
    rospy.init_node('npu_robot_yolo_action', anonymous=True, log_level=envs.log_level)
    yolo = Yolo()
    object_name = yolo.detect()
